#ifndef _KM_H
#define _KM_H    1

#include "utils.h"

#define d1           0.1519
#define a2           -0.24365
#define a3           -0.21325
#define d4           0.11235
#define d5           0.08535
#define d6           0.0819

Matrix *FK(double *angles)
{
	double C1 = cos(angles[0]);
	double C2 = cos(angles[1]);
	double C5 = cos(angles[4]);
	double C6 = cos(angles[5]);

	double S1 = sin(angles[0]);
	double S2 = sin(angles[1]);
	double S5 = sin(angles[4]);
	double S6 = sin(angles[5]);

	double C234 = cos(angles[1] + angles[2] + angles[3]);
	double S234 = sin(angles[1] + angles[2] + angles[3]);
	double C23 = cos(angles[1] + angles[2]);
	double S23 = sin(angles[1] + angles[2]);

	Matrix *r = initm(4, 4);

	r->elem[r->col*0+0] = C6 * (C1 * C234 * C5 + S1 * S5) - C1 * S234 * S6;
	r->elem[r->col*1+0] = C6 * (S1 * C234 * C5 - C1 * S5) - S1 * S234 * S6;
	r->elem[r->col*2+0] = S234 * C5 * C6 + C234 * S6;
	r->elem[r->col*3+0] = 0;

	r->elem[r->col*0+1] = -S6 * (C1 * C234 * C5 + S1 * S5) - C1 * S234 * C6;
	r->elem[r->col*1+1] = -S6 * (S1 * C234 * C5 - C1 * S5) - S1 * S234 * C6;
	r->elem[r->col*2+1] = -S6 * S234 * C5 + C234 * C6;
	r->elem[r->col*3+1] = 0;

	r->elem[r->col*0+2] = -C1 * C234 * S5 + S1 * C5;
	r->elem[r->col*1+2] = -S1 * C234 * S5 - C1 * C5;
	r->elem[r->col*2+2] = -S234 * S5;
	r->elem[r->col*3+2] = 0;

	r->elem[r->col*0+3] = d6 * (-C1 * C234 * S5 + S1 * C5) + C1 * S234 * d5 + S1 * d4 + a3 * C1 * C23 + a2 * C1 * C2;
	r->elem[r->col*1+3] = d6 * (-S1 * C234 * S5 - C1 * C5) + S1 * S234 * d5 - C1 * d4 + a3 * S1 * C23 + a2 * S1 * C2;
	r->elem[r->col*2+3] = -d6 * S234 * S5 - C234 * d5 + a3 * S23 + a2 * S2 + d1;
	r->elem[r->col*3+3] = 1;

	return r;
}

typedef struct {
	double w, x, y, z;
} quater;

typedef struct {
	double rx, ry, rz;
} rotvec;

typedef struct {
	double x, y, z, rx, ry, rz;
} pose;

rotvec RotateMatrixToVector(Matrix *mat)
{
	double w, x, y, z; //quaternion
	//from Eigen
	double t = mat->elem[0] + mat->elem[mat->col*1+1] + mat->elem[mat->col*2+2];
	// if (t > 0) {
		// t = sqrt(t + 1.0);
		// w = 0.5*t;
		// t = 0.5/t;
		// x = (mat->elem[mat->col*2+1] - mat->elem[mat->col*1+2]) * t;
		// y = (mat->elem[mat->col*0+2] - mat->elem[mat->col*2+0]) * t;
		// z = (mat->elem[mat->col*1+0] - mat->elem[mat->col*0+1]) * t;
	// }
	// else {
		int i = 0;
		if (mat->elem[mat->col*1+1] > mat->elem[mat->col*0+0])
			i = 1;
		if (mat->elem[mat->col*2+2] > mat->elem[mat->col*i+i])
			i = 2;
		int j = (i+1)%3;
		int k = (j+1)%3;

		t = sqrt(mat->elem[mat->col*i+i] - mat->elem[mat->col*j+j] - mat->elem[mat->col*k+k] + 1.0);
		if (i == 0) x = 0.5*t; else if (i == 1) y = 0.5*t; else z = 0.5*t;
		t = 0.5/t;
		w = (mat->elem[mat->col*k+j]-mat->elem[mat->col*j+k]) * t;
		double tmp = (mat->elem[mat->col*j+i] + mat->elem[mat->col*i+j]) * t;
		if (j == 0) x = tmp; else if (j == 1) y = tmp; else z = tmp;
		tmp = (mat->elem[mat->col*k+i] + mat->elem[mat->col*i+k]) * t;
		if (k == 0) x = tmp; else if (k == 1) y = tmp; else z = tmp;
	// }
	
	rotvec v;
	double n = acos(-w) * 2;
	v.rx = (-x)/sin(n/2) * n;
	v.ry = (-y)/sin(n/2) * n;
	v.rz = (-z)/sin(n/2) * n;
	
	return v;
}

Matrix *RotateVectorToMatrix(rotvec v)
{
	double magnitude = sqrt(v.rx*v.rx + v.ry*v.ry + v.rz*v.rz);
	
	double w, x, y, z; //quaternion
	w = cos(magnitude / 2);
	x = sin(magnitude / 2) * (v.rx / magnitude);
	y = sin(magnitude / 2) * (v.ry / magnitude);
	z = sin(magnitude / 2) * (v.rz / magnitude);

	Matrix *m = initm(3, 3);
	m->elem[m->col*0+0] = 1 - 2*y*y - 2*z*z;
	m->elem[m->col*0+1] = 2*x*y - 2*w*z;
	m->elem[m->col*0+2] = 2*w*y + 2*x*z;

	m->elem[m->col*1+0] = 2*x*y + 2*w*z;
	m->elem[m->col*1+1] = 1 - 2*x*x - 2*z*z;
	m->elem[m->col*1+2] = 2*y*z - 2*w*x;

	m->elem[m->col*2+0] = 2*x*z - 2*w*y;
	m->elem[m->col*2+1] = 2*w*x + 2*y*z;
	m->elem[m->col*2+2] = 1 - 2*x*x - 2*y*y;
	
	return m;
}

Matrix *IK(pose p)
{
	/*rotate matrix*/
	rotvec v;
	v.rx = p.rx;
	v.ry = p.ry;
	v.rz = p.rz;
	Matrix *tmp = RotateVectorToMatrix(v);
	Matrix *mtmp = initm(4, 4);
	for (int i = 0; i < 3; i++)
	{
		for (int j = 0; j < 3; j++)
		{
			mtmp->elem[mtmp->col*i+j] = tmp->elem[tmp->col*i+j];
		}
	}
	mtmp->elem[mtmp->col*0+3] = p.x;
	mtmp->elem[mtmp->col*1+3] = p.y;
	mtmp->elem[mtmp->col*2+3] = p.z;
	mtmp->elem[mtmp->col*3+3] = 1.0;
	/*known*/
	double nx = mtmp->elem[mtmp->col*0+0];
	double ox = mtmp->elem[mtmp->col*0+1];
	double ax = mtmp->elem[mtmp->col*0+2];
	double px = mtmp->elem[mtmp->col*0+3];

	double ny = mtmp->elem[mtmp->col*1+0];
	double oy = mtmp->elem[mtmp->col*1+1];
	double ay = mtmp->elem[mtmp->col*1+2];
	double py = mtmp->elem[mtmp->col*1+3];

	double nz = mtmp->elem[mtmp->col*2+0];
	double oz = mtmp->elem[mtmp->col*2+1];
	double az = mtmp->elem[mtmp->col*2+2];
	double pz = mtmp->elem[mtmp->col*2+3];

	/*theta*/
	double theta1[2];
	double theta2[8];
	double theta3[8];
	double theta4[8];
	double theta5[4];
	double theta6[4];
	/*theta1*/
	theta1[0] = acos(d4/sqrt(pow(d6*ay-py, 2)+pow(px-d6*ax, 2))) + atan2(px-d6*ax, d6*ay-py);
	theta1[1] = -acos(d4/sqrt(pow(d6*ay-py, 2)+pow(px-d6*ax, 2))) + atan2(px-d6*ax, d6*ay-py);
	/*theta5*/
	theta5[0] = acos(sin(theta1[0])*ax - cos(theta1[0])*ay);
	theta5[1] = -theta5[0];
	theta5[2] = acos(sin(theta1[1])*ax - cos(theta1[1])*ay);
	theta5[3] = -theta5[2];
	/*theta6*/
	theta6[0] = atan2(cos(theta1[0])*oy-sin(theta1[0])*ox, sin(theta1[0])*nx-cos(theta1[0])*ny);
	theta6[1] = theta6[0] + M_PI;
	theta6[2] = atan2(cos(theta1[1])*oy-sin(theta1[1])*ox, sin(theta1[1])*nx-cos(theta1[1])*ny);
	theta6[3] = theta6[2] + M_PI;
	/*theta3 and theta2*/
	double m, n;
	for (int i=0; i<4; i++)
	{
		int j = i<2 ? 0 : 1;
		m = d5*((cos(theta1[j])*nx+sin(theta1[j])*ny)*sin(theta6[i]) + (cos(theta1[j])*ox+sin(theta1[j])*oy)*cos(theta6[i])) - d6*(cos(theta1[j])*ax+sin(theta1[j])*ay) + cos(theta1[j])*px + sin(theta1[j])*py;
		n = d5*(nz*sin(theta6[i])+oz*cos(theta6[i])) - d6*az + pz -d1;
		theta3[i*2] = acos((pow(m,2)+pow(n,2)-pow(a2,2)-pow(a3,2)) / (2*a2*a3));
		theta3[i*2+1] = -theta3[i*2];
		theta2[i*2] = atan2(n*(a2+a3*cos(theta3[i*2])) - m*a3*sin(theta3[i*2]), m*(a2+a3*cos(theta3[i*2])) + n*a3*sin(theta3[i*2]));
		theta2[i*2+1] = atan2(n*(a2+a3*cos(theta3[i*2+1])) - m*a3*sin(theta3[i*2+1]), m*(a2+a3*cos(theta3[i*2+1])) + n*a3*sin(theta3[i*2+1]));
	}
	/*theta4*/
	theta4[0] = atan2((nz*cos(theta6[0])-oz*sin(theta6[0]))*cos(theta5[0]) - az*sin(theta5[0]), ((cos(theta1[0])*nx+sin(theta1[0])*ny)*cos(theta6[0])-(cos(theta1[0])*ox+sin(theta1[0])*oy)*sin(theta6[0]))*cos(theta5[0]) - (cos(theta1[0])*ax+sin(theta1[0])*ay)*sin(theta5[0])) - theta2[0] - theta3[0];
	theta4[1] = atan2((nz*cos(theta6[0])-oz*sin(theta6[0]))*cos(theta5[0]) - az*sin(theta5[0]), ((cos(theta1[0])*nx+sin(theta1[0])*ny)*cos(theta6[0])-(cos(theta1[0])*ox+sin(theta1[0])*oy)*sin(theta6[0]))*cos(theta5[0]) - (cos(theta1[0])*ax+sin(theta1[0])*ay)*sin(theta5[0])) - theta2[1] - theta3[1];
	theta4[2] = atan2((nz*cos(theta6[1])-oz*sin(theta6[1]))*cos(theta5[1]) - az*sin(theta5[1]), ((cos(theta1[0])*nx+sin(theta1[0])*ny)*cos(theta6[1])-(cos(theta1[0])*ox+sin(theta1[0])*oy)*sin(theta6[1]))*cos(theta5[1]) - (cos(theta1[0])*ax+sin(theta1[0])*ay)*sin(theta5[1])) - theta2[2] - theta3[2];
	theta4[3] = atan2((nz*cos(theta6[1])-oz*sin(theta6[1]))*cos(theta5[1]) - az*sin(theta5[1]), ((cos(theta1[0])*nx+sin(theta1[0])*ny)*cos(theta6[1])-(cos(theta1[0])*ox+sin(theta1[0])*oy)*sin(theta6[1]))*cos(theta5[1]) - (cos(theta1[0])*ax+sin(theta1[0])*ay)*sin(theta5[1])) - theta2[3] - theta3[3];
	theta4[4] = atan2((nz*cos(theta6[2])-oz*sin(theta6[2]))*cos(theta5[2]) - az*sin(theta5[2]), ((cos(theta1[1])*nx+sin(theta1[1])*ny)*cos(theta6[2])-(cos(theta1[1])*ox+sin(theta1[1])*oy)*sin(theta6[2]))*cos(theta5[2]) - (cos(theta1[1])*ax+sin(theta1[1])*ay)*sin(theta5[2])) - theta2[4] - theta3[4];
	theta4[5] = atan2((nz*cos(theta6[2])-oz*sin(theta6[2]))*cos(theta5[2]) - az*sin(theta5[2]), ((cos(theta1[1])*nx+sin(theta1[1])*ny)*cos(theta6[2])-(cos(theta1[1])*ox+sin(theta1[1])*oy)*sin(theta6[2]))*cos(theta5[2]) - (cos(theta1[1])*ax+sin(theta1[1])*ay)*sin(theta5[2])) - theta2[5] - theta3[5];
	theta4[6] = atan2((nz*cos(theta6[3])-oz*sin(theta6[3]))*cos(theta5[3]) - az*sin(theta5[3]), ((cos(theta1[1])*nx+sin(theta1[1])*ny)*cos(theta6[3])-(cos(theta1[1])*ox+sin(theta1[1])*oy)*sin(theta6[3]))*cos(theta5[3]) - (cos(theta1[1])*ax+sin(theta1[1])*ay)*sin(theta5[3])) - theta2[6] - theta3[6];
	theta4[7] = atan2((nz*cos(theta6[3])-oz*sin(theta6[3]))*cos(theta5[3]) - az*sin(theta5[3]), ((cos(theta1[1])*nx+sin(theta1[1])*ny)*cos(theta6[3])-(cos(theta1[1])*ox+sin(theta1[1])*oy)*sin(theta6[3]))*cos(theta5[3]) - (cos(theta1[1])*ax+sin(theta1[1])*ay)*sin(theta5[3])) - theta2[7] - theta3[7];
	
	Matrix *solu = initm(8, 6);
	for (int i=0; i<4; i++)
	{
		solu->elem[solu->col*(0+i)] = theta1[0];
		solu->elem[solu->col*(4+i)] = theta1[1];
		solu->elem[solu->col*(i*2)+4] = theta5[i];
		solu->elem[solu->col*(i*2+1)+4] = theta5[i];
		solu->elem[solu->col*(i*2)+5] = theta6[i];
		solu->elem[solu->col*(i*2+1)+5] = theta6[i];
	}
	for (int i=0; i<8; i++)
	{
		solu->elem[solu->col*i+2] = theta3[i];
		solu->elem[solu->col*i+1] = theta2[i];
		solu->elem[solu->col*i+3] = theta4[i];
	}
	
	return solu;
}

Matrix *IK_old(pose p, double theta6)
{
	const double d11 = 0.1519;
	const double a22 = 0.24365;
	const double a33 = 0.21325;
	const double d44 = 0.11235;
	const double d55 = 0.08535;
	const double d66 = 0.0819;

	/*sita*/
	double sita1[2];
	double sita2[8];
	double sita3[8];
	double sita4[8];
	double sita5[4];
	double sita6[4];
	double sita234[4];
	/*temp*/
	double a, b, c;
	double t1, t2;
	/*rotate matrix*/
	rotvec v;
	v.rx = p.rx;
	v.ry = p.ry;
	v.rz = p.rz;
	Matrix *tmp = RotateVectorToMatrix(v);
	Matrix *m = initm(4, 4);
	for (int i = 0; i < 3; i++)
	{
		for (int j = 0; j < 3; j++)
		{
			m->elem[m->col*i+j] = tmp->elem[tmp->col*i+j];
		}
	}
	m->elem[m->col*0+3] = p.x;
	m->elem[m->col*1+3] = p.y;
	m->elem[m->col*2+3] = p.z;
	m->elem[m->col*3+3] = 1.0;
	/*known*/
	double r11 = m->elem[m->col*0+0];
	double r12 = m->elem[m->col*0+1];
	double r13 = m->elem[m->col*0+2];
	double px = m->elem[m->col*0+3];

	double r21 = m->elem[m->col*1+0];
	double r22 = m->elem[m->col*1+1];
	double r23 = m->elem[m->col*1+2];
	double py = m->elem[m->col*1+3];

	double r31 = m->elem[m->col*2+0];
	double r32 = m->elem[m->col*2+1];
	double r33 = m->elem[m->col*2+2];
	double pz = m->elem[m->col*2+3];

	Matrix *solu = initm(8, 6);
	/*sita1*/
	a = px - d66 * r13;
	b = py - d66 * r23;
	if (threshold(a) && threshold(b))
	{
		printf("ERROR IK of sita1: 无法通过当前方法解得sita1\n");
		exit(0);
	}
	c = -d44;

	sita1[0] = atan2(b, a) - atan2(c, sqrt(a * a + b * b - c * c));
	sita1[1] = atan2(b, a) - atan2(c, -sqrt(a * a + b * b - c * c));
	for (int i = 0; i < 4; i++)
	{
		solu->elem[solu->col*i+0] = sita1[0];
	}
	for (int i = 4; i < 8; i++)
	{
		solu->elem[solu->col*i+0] = sita1[1];
	}
	/*sita5*/
		/*sita1[0]*/
	double c5;
	double s5;
	c5 = sin(sita1[0]) * r13 - cos(sita1[0]) * r23;
	if (threshold(c5-1))
	{
		sita5[0] = 0;
		sita5[1] = 0;
	}
	else
	{
		s5 = sqrt(1 - c5 * c5);//正
		sita5[0] = atan2(s5, c5);
		s5 = -s5;//负
		sita5[1] = atan2(s5, c5);
	}
	/*sita1[1]*/
	c5 = sin(sita1[1]) * r13 - cos(sita1[1]) * r23;
	if (threshold(c5-1))
	{
		sita5[2] = 0;
		sita5[3] = 0;
	}
	else
	{
		s5 = sqrt(1 - c5 * c5);//正
		sita5[2] = atan2(s5, c5);
		s5 = -s5;//负
		sita5[3] = atan2(s5, c5);
	}

	solu->elem[solu->col*0+4] = sita5[0];
	solu->elem[solu->col*1+4] = sita5[0];
	solu->elem[solu->col*2+4] = sita5[1];
	solu->elem[solu->col*3+4] = sita5[1];
	solu->elem[solu->col*4+4] = sita5[2];
	solu->elem[solu->col*5+4] = sita5[2];
	solu->elem[solu->col*6+4] = sita5[3];
	solu->elem[solu->col*7+4] = sita5[3];

	/*sita6*/
	double C6, S6;
	//sita1[0]->sita5[0]
	if (threshold(sita5[0]))
	{//检查奇异性
		sita6[0] = theta6;
	}
	else
	{
		C6 = -(cos(sita1[0]) * r21 - sin(sita1[0]) * r11) / sin(sita5[0]);
		S6 = (cos(sita1[0]) * r22 - sin(sita1[0]) * r12) / sin(sita5[0]);
		if (threshold(C6) && threshold(S6))
		{
			printf("ERROR IN IK of sita6 : CheckZeroThresh(C6) && CheckZeroThresh(S6)\n");
			exit(0);
		}
		sita6[0] = atan2(S6, C6);
	}
	//sita1[0]->sita5[1]
	if (threshold(sita5[1]))
	{//检查奇异性
		sita6[1] = theta6;
	}
	else
	{
		C6 = -(cos(sita1[0]) * r21 - sin(sita1[0]) * r11) / sin(sita5[1]);
		S6 = (cos(sita1[0]) * r22 - sin(sita1[0]) * r12) / sin(sita5[1]);
		if (threshold(C6) && threshold(S6))
		{
			printf("ERROR IN IK of sita6 : CheckZeroThresh(C6) && CheckZeroThresh(S6)\n");
			exit(0);
		}
		sita6[1] = atan2(S6, C6);
	}
	//sita1[1]->sita5[2]
	if (threshold(sita5[2]))
	{//检查奇异性
		sita6[2] = theta6;
	}
	else
	{
		C6 = -(cos(sita1[1]) * r21 - sin(sita1[1]) * r11) / sin(sita5[2]);
		S6 = (cos(sita1[1]) * r22 - sin(sita1[1]) * r12) / sin(sita5[2]);
		if (threshold(C6) && threshold(S6))
		{
			printf("ERROR IN IK of sita6 : CheckZeroThresh(C6) && CheckZeroThresh(S6)\n");
			exit(0);
		}
		sita6[2] = atan2(S6, C6);
	}
	//sita1[1]->sita5[3]
	if (threshold(sita5[3]))
	{//检查奇异性
		sita6[3] = theta6;
	}
	else
	{
		C6 = -(cos(sita1[1]) * r21 - sin(sita1[1]) * r11) / sin(sita5[3]);
		S6 = (cos(sita1[1]) * r22 - sin(sita1[1]) * r12) / sin(sita5[3]);
		if (threshold(C6) && threshold(S6))
		{
			printf("ERROR IN IK of sita6 : CheckZeroThresh(C6) && CheckZeroThresh(S6)\n");
			exit(0);
		}
		sita6[3] = atan2(S6, C6);
	}

	solu->elem[solu->col*0+5] = sita6[0];
	solu->elem[solu->col*1+5] = sita6[0];
	solu->elem[solu->col*2+5] = sita6[1];
	solu->elem[solu->col*3+5] = sita6[1];
	solu->elem[solu->col*4+5] = sita6[2];
	solu->elem[solu->col*5+5] = sita6[2];
	solu->elem[solu->col*6+5] = sita6[3];
	solu->elem[solu->col*7+5] = sita6[3];

	/*sita(2+3+4)*/
	double S234;
	double C234;

	/*sita1[0]->sita5[0] sita6[0]*/
	S234 = cos(sita5[0]) * (cos(sita6[0]) * r31 - sin(sita6[0]) * r32) - sin(sita5[0]) * r33;
	C234 = cos(sita6[0]) * r32 + sin(sita6[0]) * r31;
	sita234[0] = atan2(S234, C234);

	/*sita1[0]->sita5[1] sita6[1]*/
	S234 = cos(sita5[1]) * (cos(sita6[1]) * r31 - sin(sita6[1]) * r32) - sin(sita5[1]) * r33;
	C234 = cos(sita6[1]) * r32 + sin(sita6[1]) * r31;
	sita234[1] = atan2(S234, C234);

	/*sita1[1]->sita5[2] sita6[2]*/
	S234 = cos(sita5[2]) * (cos(sita6[2]) * r31 - sin(sita6[2]) * r32) - sin(sita5[2]) * r33;
	C234 = cos(sita6[2]) * r32 + sin(sita6[2]) * r31;
	sita234[2] = atan2(S234, C234);

	/*sita1[1]->sita5[3] sita6[3]*/
	S234 = cos(sita5[3]) * (cos(sita6[3]) * r31 - sin(sita6[3]) * r32) - sin(sita5[3]) * r33;
	C234 = cos(sita6[3]) * r32 + sin(sita6[3]) * r31;
	sita234[3] = atan2(S234, C234);

	/*sita3*/
	double C3, S3;
	/*sita1[0]->sita5[0]->sita6[0]->sita234[0]*/
	t1 = cos(sita1[0]) * px + sin(sita1[0]) * py - d55 * sin(sita234[0]) + d66 * sin(sita5[0]) * cos(sita234[0]);
	t2 = pz - d11 + d55 * cos(sita234[0]) + d66 * sin(sita5[0]) * sin(sita234[0]);
	C3 = (t1 * t1 + t2 * t2 - a33 * a33 - a22 * a22) / (2 * a22 * a33);
	if (threshold(C3-1))
	{
		sita3[0] = 0;
		sita3[1] = 0;
	}
	else
	{
		S3 = sqrt(1 - C3 * C3);
		sita3[0] = atan2(S3, C3);
		sita3[1] = atan2(-S3, C3);
	}
	/*sita1[0]->sita5[1]->sita6[1]->sita234[1]*/
	t1 = cos(sita1[0]) * px + sin(sita1[0]) * py - d55 * sin(sita234[1]) + d66 * sin(sita5[1]) * cos(sita234[1]);
	t2 = pz - d11 + d55 * cos(sita234[1]) + d66 * sin(sita5[1]) * sin(sita234[1]);
	C3 = (t1 * t1 + t2 * t2 - a33 * a33 - a22 * a22) / (2 * a22 * a33);
	if (threshold(C3-1))
	{
		sita3[2] = 0;
		sita3[3] = 0;
	}
	else
	{
		S3 = sqrt(1 - C3 * C3);
		sita3[2] = atan2(S3, C3);
		sita3[3] = atan2(-S3, C3);
	}
	/*sita1[1]->sita5[2]->sita6[2]->sita234[2]*/
	t1 = cos(sita1[1]) * px + sin(sita1[1]) * py - d55 * sin(sita234[2]) + d66 * sin(sita5[2]) * cos(sita234[2]);
	t2 = pz - d11 + d55 * cos(sita234[2]) + d66 * sin(sita5[2]) * sin(sita234[2]);
	C3 = (t1 * t1 + t2 * t2 - a33 * a33 - a22 * a22) / (2 * a22 * a33);
	if (threshold(C3-1))
	{
		sita3[4] = 0;
		sita3[5] = 0;
	}
	else
	{
		S3 = sqrt(1 - C3 * C3);
		sita3[4] = atan2(S3, C3);
		sita3[5] = atan2(-S3, C3);
	}
	/*sita1[1]->sita5[3]->sita6[3]->sita234[3]*/
	t1 = cos(sita1[1]) * px + sin(sita1[1]) * py - d55 * sin(sita234[3]) + d66 * sin(sita5[3]) * cos(sita234[3]);
	t2 = pz - d11 + d55 * cos(sita234[3]) + d66 * sin(sita5[3]) * sin(sita234[3]);
	C3 = (t1 * t1 + t2 * t2 - a33 * a33 - a22 * a22) / (2 * a22 * a33);
	if (threshold(C3-1))
	{
		sita3[6] = 0;
		sita3[7] = 0;
	}
	else
	{
		S3 = sqrt(1 - C3 * C3);
		sita3[6] = atan2(S3, C3);
		sita3[7] = atan2(-S3, C3);
	}

	solu->elem[solu->col*0+2] = sita3[0];
	solu->elem[solu->col*1+2] = sita3[1];
	solu->elem[solu->col*2+2] = sita3[2];
	solu->elem[solu->col*3+2] = sita3[3];
	solu->elem[solu->col*4+2] = sita3[4];
	solu->elem[solu->col*5+2] = sita3[5];
	solu->elem[solu->col*6+2] = sita3[6];
	solu->elem[solu->col*7+2] = sita3[7];

	/*sita2*/
	double C2, S2;
	/*sita1[0]->sita5[0]->sita6[0]->sita234[0]->sita3[0]*/
	t1 = cos(sita1[0]) * px + sin(sita1[0]) * py - d55 * sin(sita234[0]) + d66 * sin(sita5[0]) * cos(sita234[0]);
	t2 = pz - d11 + d55 * cos(sita234[0]) + d66 * sin(sita5[0]) * sin(sita234[0]);
	C2 = ((a33 * cos(sita3[0]) + a22) * t1 + t2 * a33 * sin(sita3[0])) / -(a22 * a22 + a33 * a33 + 2 * a22 * a33 * cos(sita3[0]));
	S2 = (t2 + a33 * sin(sita3[0]) * C2) / -(a33 * cos(sita3[0]) + a22);
	sita2[0] = atan2(S2, C2);
	/*sita1[0]->sita5[0]->sita6[0]->sita234[0]->sita3[1]*/
	t1 = cos(sita1[0]) * px + sin(sita1[0]) * py - d55 * sin(sita234[0]) + d66 * sin(sita5[0]) * cos(sita234[0]);
	t2 = pz - d11 + d55 * cos(sita234[0]) + d66 * sin(sita5[0]) * sin(sita234[0]);
	C2 = ((a33 * cos(sita3[1]) + a22) * t1 + t2 * a33 * sin(sita3[1])) / -(a22 * a22 + a33 * a33 + 2 * a22 * a33 * cos(sita3[1]));
	S2 = (t2 + a33 * sin(sita3[1]) * C2) / -(a33 * cos(sita3[1]) + a22);
	sita2[1] = atan2(S2, C2);
	/*sita1[0]->sita5[1]->sita6[1]->sita234[1]->sita3[2]*/
	t1 = cos(sita1[0]) * px + sin(sita1[0]) * py - d55 * sin(sita234[1]) + d66 * sin(sita5[1]) * cos(sita234[1]);
	t2 = pz - d11 + d55 * cos(sita234[1]) + d66 * sin(sita5[1]) * sin(sita234[1]);
	C2 = ((a33 * cos(sita3[2]) + a22) * t1 + t2 * a33 * sin(sita3[2])) / -(a22 * a22 + a33 * a33 + 2 * a22 * a33 * cos(sita3[2]));
	S2 = (t2 + a33 * sin(sita3[2]) * C2) / -(a33 * cos(sita3[2]) + a22);
	sita2[2] = atan2(S2, C2);
	/*sita1[0]->sita5[1]->sita6[1]->sita234[1]->sita3[3]*/
	t1 = cos(sita1[0]) * px + sin(sita1[0]) * py - d55 * sin(sita234[1]) + d66 * sin(sita5[1]) * cos(sita234[1]);
	t2 = pz - d11 + d55 * cos(sita234[1]) + d66 * sin(sita5[1]) * sin(sita234[1]);
	C2 = ((a33 * cos(sita3[3]) + a22) * t1 + t2 * a33 * sin(sita3[3])) / -(a22 * a22 + a33 * a33 + 2 * a22 * a33 * cos(sita3[3]));
	S2 = (t2 + a33 * sin(sita3[3]) * C2) / -(a33 * cos(sita3[3]) + a22);
	sita2[3] = atan2(S2, C2);

	/*sita1[1]->sita5[2]->sita6[2]->sita234[2]->sita3[4]*/
	t1 = cos(sita1[1]) * px + sin(sita1[1]) * py - d55 * sin(sita234[2]) + d66 * sin(sita5[2]) * cos(sita234[2]);
	t2 = pz - d11 + d55 * cos(sita234[2]) + d66 * sin(sita5[2]) * sin(sita234[2]);
	C2 = ((a33 * cos(sita3[4]) + a22) * t1 + t2 * a33 * sin(sita3[4])) / -(a22 * a22 + a33 * a33 + 2 * a22 * a33 * cos(sita3[4]));
	S2 = (t2 + a33 * sin(sita3[4]) * C2) / -(a33 * cos(sita3[4]) + a22);
	sita2[4] = atan2(S2, C2);
	/*sita1[1]->sita5[2]->sita6[2]->sita234[2]->sita3[5]*/
	t1 = cos(sita1[1]) * px + sin(sita1[1]) * py - d55 * sin(sita234[2]) + d66 * sin(sita5[2]) * cos(sita234[2]);
	t2 = pz - d11 + d55 * cos(sita234[2]) + d66 * sin(sita5[2]) * sin(sita234[2]);
	C2 = ((a33 * cos(sita3[5]) + a22) * t1 + t2 * a33 * sin(sita3[5])) / -(a22 * a22 + a33 * a33 + 2 * a22 * a33 * cos(sita3[5]));
	S2 = (t2 + a33 * sin(sita3[5]) * C2) / -(a33 * cos(sita3[5]) + a22);
	sita2[5] = atan2(S2, C2);
	/*sita1[1]->sita5[3]->sita6[3]->sita234[3]->sita3[6]*/
	t1 = cos(sita1[1]) * px + sin(sita1[1]) * py - d55 * sin(sita234[3]) + d66 * sin(sita5[3]) * cos(sita234[3]);
	t2 = pz - d11 + d55 * cos(sita234[3]) + d66 * sin(sita5[3]) * sin(sita234[3]);
	C2 = ((a33 * cos(sita3[6]) + a22) * t1 + t2 * a33 * sin(sita3[6])) / -(a22 * a22 + a33 * a33 + 2 * a22 * a33 * cos(sita3[6]));
	S2 = (t2 + a33 * sin(sita3[6]) * C2) / -(a33 * cos(sita3[6]) + a22);
	sita2[6] = atan2(S2, C2);
	/*sita1[1]->sita5[3]->sita6[3]->sita234[3]->sita3[7]*/
	t1 = cos(sita1[1]) * px + sin(sita1[1]) * py - d55 * sin(sita234[3]) + d66 * sin(sita5[3]) * cos(sita234[3]);
	t2 = pz - d11 + d55 * cos(sita234[3]) + d66 * sin(sita5[3]) * sin(sita234[3]);
	C2 = ((a33 * cos(sita3[7]) + a22) * t1 + t2 * a33 * sin(sita3[7])) / -(a22 * a22 + a33 * a33 + 2 * a22 * a33 * cos(sita3[7]));
	S2 = (t2 + a33 * sin(sita3[7]) * C2) / -(a33 * cos(sita3[7]) + a22);
	sita2[7] = atan2(S2, C2);

	solu->elem[solu->col*0+1] = sita2[0];
	solu->elem[solu->col*1+1] = sita2[1];
	solu->elem[solu->col*2+1] = sita2[2];
	solu->elem[solu->col*3+1] = sita2[3];
	solu->elem[solu->col*4+1] = sita2[4];
	solu->elem[solu->col*5+1] = sita2[5];
	solu->elem[solu->col*6+1] = sita2[6];
	solu->elem[solu->col*7+1] = sita2[7];

	/*sita4*/
		/*sita234[0]->sita3[0]->sita2[0]*/
	sita4[0] = sita234[0] - sita3[0] - sita2[0];
	/*sita234[0]->sita3[1]->sita2[1]*/
	sita4[1] = sita234[0] - sita3[1] - sita2[1];
	/*sita234[1]->sita3[2]->sita2[2]*/
	sita4[2] = sita234[1] - sita3[2] - sita2[2];
	/*sita234[1]->sita3[3]->sita2[3]*/
	sita4[3] = sita234[1] - sita3[3] - sita2[3];
	/*sita234[2]->sita3[4]->sita2[4]*/
	sita4[4] = sita234[2] - sita3[4] - sita2[4];
	/*sita234[2]->sita3[5]->sita2[5]*/
	sita4[5] = sita234[2] - sita3[5] - sita2[5];
	/*sita234[3]->sita3[6]->sita2[6]*/
	sita4[6] = sita234[3] - sita3[6] - sita2[6];
	/*sita234[3]->sita3[7]->sita2[7]*/
	sita4[7] = sita234[3] - sita3[7] - sita2[7];

	solu->elem[solu->col*0+3] = sita4[0];
	solu->elem[solu->col*1+3] = sita4[1];
	solu->elem[solu->col*2+3] = sita4[2];
	solu->elem[solu->col*3+3] = sita4[3];
	solu->elem[solu->col*4+3] = sita4[4];
	solu->elem[solu->col*5+3] = sita4[5];
	solu->elem[solu->col*6+3] = sita4[6];
	solu->elem[solu->col*7+3] = sita4[7];

	return solu;
}

pose tranpose(double *array)
{
	pose p;
	p.x = array[0];
	p.y = array[1];
	p.z = array[2];
	p.rx = array[3];
	p.ry = array[4];
	p.rz = array[5];
	return p;
}

int *verify(Matrix *m, pose p)
{
	int *valid = (int*)malloc(sizeof(int)*9);
	memset(valid, 0, sizeof(int)*9);

	for (int i=0; i<m->row; i++)
	{
		for (int j=0; j<m->col; j++)
		{
			if (isnan(m->elem[m->col*i+j]))
			{
				printf("the solu is nan\n");
				valid[i] = -1;
				break;
			}
		}
		if (valid[i] == -1)
		{
			break;
		}
		else
		{
			Matrix *tmp = FK(m->elem+i*m->col);
			rotvec v = RotateMatrixToVector(tmp);
			if (!threshold(tmp->elem[3]-p.x) || !threshold(tmp->elem[7]-p.y) || !threshold(tmp->elem[11]-p.z) || !threshold(v.rx-p.rx) || !threshold(v.ry-p.ry) || !threshold(v.rz-p.rz))
			{
				printf("the solu is wrong\n");
				printf("pose: %lf %lf %lf %lf %lf %lf\n", p.x,p.y,p.z,p.rx,p.ry,p.rz);
				printf("solu: %lf %lf %lf %lf %lf %lf\n", tmp->elem[3],tmp->elem[7],tmp->elem[11],v.rx,v.ry,v.rz);
				valid[i] = -2;
			}
		}
	}
	valid[8] = 8;
	for (int i=0; i<8; i++)
	{
		if (valid[i] != 0)
		{
			valid[8]--;
		}
	}

	return valid;
}

double dist2p(pose p1, pose p2)
{
	return sqrt((p1.x-p2.x)*(p1.x-p2.x)+(p1.y-p2.y)*(p1.y-p2.y)+(p1.z-p2.z)*(p1.z-p2.z));
}

double *findOptimalSolu(pose p, double *curjoint)
{
	Matrix *m = IK(p);
	int *valid = verify(m, p);
	/* 待返回的最优解 */
	double *optimal = (double*)malloc(sizeof(double)*7);
	memset(optimal, 0, sizeof(double)*7);
	/* 考虑加减2PI的等效角 */
	double equjoint[6];
	if (valid[8] > 0)
	{
		int optimal_index = 0;
		double dist_min = 10000;
		for (int i=0; i<8; i++)
		{
			if (valid[i] < 0) continue;
			double tmp = 0;
			for (int j=0; j<6; j++)
			{
				equjoint[j] = m->elem[m->col*i+j]>0 ? m->elem[m->col*i+j]-2*M_PI : m->elem[m->col*i+j]+2*M_PI;
				if (fabs(equjoint[j]-curjoint[j]) < fabs(m->elem[m->col*i+j]-curjoint[j]))
					m->elem[m->col*i+j] = equjoint[j];
				tmp += fabs(m->elem[m->col*i+j]-curjoint[j]);
			}
			if (tmp < dist_min)
			{
				dist_min = tmp;
				optimal_index = i;
			}
		}
		for (int i=0; i<6; i++)
			optimal[i] = m->elem[m->col*optimal_index+i];
		optimal[6] = 1;
	}
	else {
		printf("can't find valid IK solu.\n");
	}
	freem(m);
	free(valid);
	return optimal;
}

void find()
{
	double target[6] = {-0.47964, -0.04361, 0.170, 2.2378, 2.059, 0.009};
	double *solu = findOptimalSolu(tranpose(target), urmes.q_actual);
	for (int i=0; i<7; i++)
	{
		printf("%lf ", r2d(solu[i]));
	}
	printf("\n");
	free(solu);
}

void iktest()
{
	double TESTPOSE[30] = {-0.06378, -0.20125, 0.13728, 0.192, 3.109, 0.036, 0, -0.19425, 0.69415, 0, 2.221, -2.221, -0.20115, -0.22787, 0.2815, 1.147, 4.404, -1.014, -0.19692, -0.39847, 0.16828, 0.934, -1.363, -0.943, -0.19716, -0.43491, 0.26972, 1.820, 3.734, -1.794};
	for (int i=0; i<6; i+=6)
	{
		Matrix *m = IK(tranpose(TESTPOSE+i));
		printm(m);
		int *va = verify(m, tranpose(TESTPOSE+i));
		for (int j=0; j<9; j++) printf("%d ", va[j]);
		free(va);
		freem(m);
		printf("\n");
		// Matrix *m2 = IK_old(tranpose(TESTPOSE+i), 0);
		// int *va2 = verify(m2, tranpose(TESTPOSE+i));
		// for (int j=0; j<9; j++) printf("%d ", va2[j]);
		// free(va2);
		// freem(m2);
		// printf("\n\n");
	}
}

#endif /* <km.h> included.  */